import rclpy
from geometry_msgs.msg import PoseStamped,Pose #PoseStamped：ROS 2 的标准消息类型，用于表示带时间戳和坐标系的三维位姿（位置 + 方向）。
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult #BasicNavigator：Nav2 提供的简化接口，封装了导航任务（如设置初始位姿、发送目标点）。
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener,Buffer #TF坐标监听器
from tf_transformations import euler_from_quaternion,quaternion_from_euler #四元数转欧拉角,欧拉角转四元数
import math
from autopatol_interfaces.srv import SpeechText
from sensor_msgs.msg import Image   #图像话题消息接口
from cv_bridge import CvBridge #转换图像格式
import cv2 #保存图像

class PartolNode(BasicNavigator):
    def __init__(self, node_name='partol_robot'):
        super().__init__(node_name)
        #声明巡航点相关参数
        self.declare_parameter('initial_point',[0.0, 0.0, 0.0]) #初始点参数声明
        self.declare_parameter('target_points',[0.0, 0.0, 0.0,    1.0,1.0,1.57]) #路点参数声明
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value

        self.buffer_ = Buffer()
        #创建一个坐标变换（TF）的监听器,TransformListener 的构造函数需要两个参数：buffer 和 node,代码中传递的是 self.buffer_ 和 self
        self.listener_=TransformListener(self.buffer_,self)

        #创造一个客户端，调用服务
        self.speech_client_ = self.create_client(SpeechText,'speech_text')

        #声明图像的相关参数
        self.declare_parameter('img_save_path','')   #初始化图像存储路径参数
        self.img_save_path_ = self.get_parameter('img_save_path').value  #获取图像存储路径，空为默认路径
        self.cv_bridge_ = CvBridge()
        self.latest_img_ = None #空的实例变量
        self.img_sub_ = self.create_subscription(Image,'/camera_sensor/image_raw',self.img_callback,10) #监听相机发布的图像数据，收到新图像时就交给回调函数处理

    def img_callback(self,msg):
        self.latest_img_ = msg


    def record_img(self):
        """
        记录图像
        """
        if self.latest_img_ is not None:
            pose = self.get_current_pose()
            cv_image = self.cv_bridge_.imgmsg_to_cv2(self.latest_img_,desired_encoding='bgr8')
            cv2.imwrite(f'{self.img_save_path_}image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image)



    def get_pose_by_xyyaw(self,x,y,yaw):
        """
        return PoseStamped对象
        """
        pose = PoseStamped()                
        pose.header.frame_id = "map"
        pose.header.stamp = self.get_clock().now().to_msg()  #如果没有这个时间戳，将不能正常得到config文件的参数
        pose.pose.position.x =x
        pose.pose.position.y =y
        quat = quaternion_from_euler(0,0,yaw)
        pose.pose.orientation.w = quat[3]

        return pose
        

    def init_robot_pose(self):
        """
        初始化机器人的位姿
        """
        self.initial_point_ = self.get_parameter('initial_point').value
        init_pose = self.get_pose_by_xyyaw(self.initial_point_[0],self.initial_point_[1],self.initial_point_[2])
        self.setInitialPose(init_pose)  #将初始位姿发布
        self.waitUntilNav2Active() #等待导航激活可用

    def get_target_points(self):
        """
        通过参数获取目标点集合
        """
        points = []
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)):
            x=self.target_points_[index*3]    
            y=self.target_points_[index*3+1]
            yaw=self.target_points_[index*3+2] 
            points.append([x,y,yaw])
            self.get_logger().info(f"获取到目标点{index}->{x},{y},{yaw}")
        return points


    def nav_to_pose(self,target_point):
        """
        导航到目标点
        """
        self.waitUntilNav2Active()
        self.goToPose(target_point)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
        result = self.getResult()
        self.get_logger().info(f'导航结果：{result}')

    def get_current_pose(self):
        """
        通过TF获取当前位姿
        """
        while rclpy.ok():
            try:
                tf = self.buffer_.lookup_transform(
                    'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
                transform = tf.transform
                rotation_euler = euler_from_quaternion([
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w
                ])
                self.get_logger().info(
                    f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
                return transform
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')


    def speech_text(self, text):
        """
        调用服务合成语音
        """
        while not self.speech_client_.wait_for_service(timeout_sec=1.0):
            self.get_logger().info(f'等待语言服务上线...')

        request = SpeechText.Request()
        request.text = text
        future = self.speech_client_.call_async(request)
        rclpy.spin_until_future_complete(self,future)
        if future.result() is not None:
            response= future.result()
            if response.result == True:
                self.get_logger().info(f'语音合成成功{text}')
            else:
                self.get_logger().warn(f'语音合成失败')
        else:
            self.get_logger().warn(f'未收到语音服务响应')



def main():
    rclpy.init()
    partol = PartolNode()
    partol.speech_text('正在初始化机器人位置')
    #rclpy.spin(partol)     #仅仅是为了得到param的yaml文件 ros2 param dump /partol_robot
    partol.init_robot_pose()
    partol.speech_text('初始化机器人位置完成')


    while rclpy.ok():
        points = partol.get_target_points()
        for point in points:
            x,y,yaw = point[0],point[1],point[2]
            target_pose = partol.get_pose_by_xyyaw(x,y,yaw)
            partol.speech_text(f'正在准备前往{x},{y}目标点')
            partol.nav_to_pose(target_pose)
            partol.speech_text(f'已经到达{x},{y}目标点') 
            partol.record_img()
            partol.speech_text(f'图像记录完成') 


    rclpy.shutdown()
    
